import numpy as np
import matplotlib.pyplot as plt

N = 300
T = 0.05

shape = 2.4  # 参数名称，用于参考轨迹生成
dm1 = 25
dm2 = 25  # 没有实际意义，只是参数名称
dn1 = 3.6
dn2 = 3.6  # 没有实际意义，只是参数名称
Xs1 = 60
Xs2 = 120  # 参数名称
phi_ref = np.zeros(N)
Y_ref = np.zeros(N)
X_phi = np.arange(0.05, 150, 0.5)

X_ref = X_phi



for p in range(N):
    # 双移线追踪
    r1 = shape / dm1 * (X_ref[p] - Xs1) - shape / 2
    r2 = shape / dm2 * (X_ref[p] - Xs2) - shape / 2
    Y_ref[p] = dn1 / 2 * (1 + np.tanh(r1)) - dn2 / 2 * (1 + np.tanh(r2))
    phi_ref[p] = np.arctan(dn1 * (1 / np.cosh(r1))**2 * (1.2 / dm1) - dn2 * (1 / np.cosh(r2))**2 * (1.2 / dm2))

plt.figure(1)
plt.plot(X_ref, Y_ref, 'b--', linewidth=1)
plt.title('双移线图例')
plt.xlabel('横向位置X（米）')
plt.axis([0, 150, -2, 4])  # 直线工况
#plt.axis([-50, 50, 0, 100])  # 圆形工况
plt.ylabel('纵向位置Y（米）')
plt.legend(['参考轨迹'])
plt.grid(True)
plt.show()
